#include <stepper/uln2003apg.h>

#include <avr/interrupt.h>
#include <avr/io.h>

#define MAX_STEPPER 16

#if defined(__AVR_ATmega328P__)
 #define __STEPPER_VECTOR                     TIMER2_COMPA_vect
 #define __STEPPER_TIMER_PRESCALE_REGISTER    TCCR2B
 #define __STEPPER_TIMER_SCALE_REGISTER       OCR2A
 #define __STEPPER_TIMER_COUNTER_REGISTER     TCCR2A
 #define __STEPPER_TIMER_MAX_COUNTER_REGISTER TIMSK2
 #define __STEPPER_TIMER_MAX_COUNTER_VALUE    _BV(OCIE2A)
 #define __STEPPER_TIMER_COUNTER_VALUE        _BV(WGM21);
#else
 #define __STEPPER_VECTOR                     TIMER3_COMPA_vect
 #define __STEPPER_TIMER_PRESCALE_REGISTER    TCCR3B
 #define __STEPPER_TIMER_SCALE_REGISTER       OCR3A
 #define __STEPPER_TIMER_COUNTER_REGISTER     TCCR3A
 #define __STEPPER_TIMER_MAX_COUNTER_REGISTER TIMSK3
 #define __STEPPER_TIMER_MAX_COUNTER_VALUE    _BV(OCIE3A)
 #define __STEPPER_TIMER_COUNTER_VALUE        _BV(WGM31);
#endif


namespace stepper
{


struct _uln2003apg_status
{
  _uln2003apg_status(void)
    : stepper(0x0)
    , count(0)
    , step(0)
  {}

  uln2003apg volatile* stepper;
  int count;
  int step;
  int volatile stamp;
  bool volatile activated;
  bool volatile clockwise;

  int volatile pin1;
  int volatile pin2;
  int volatile pin3;
  int volatile pin4;
};


static _uln2003apg_status volatile _status[MAX_STEPPER];
static unsigned volatile _status_max = 0;
int const uln2003apg_lookup[8] =
{ B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001 };



void _update_uln2003apg(void)
{
  for (unsigned i = 0; i < stepper::_status_max; ++i)
  {
    ++stepper::_status[i].count;

    if (stepper::_status[i].activated &&
        (stepper::_status[i].count >=
         stepper::_status[i].stamp))
    {
//      Serial.print(i);
//      Serial.print(": ");
//      Serial.println(stepper::_status[i].step);
//      Serial.print(_status[i].pin1);
//      Serial.print(", ");
//      Serial.print(_status[i].pin2);
//      Serial.print(", ");
//      Serial.print(_status[i].pin3);
//      Serial.print(", ");
//      Serial.print(_status[i].pin4);
      digitalWrite(
            _status[i].pin1,
            bitRead(uln2003apg_lookup[stepper::_status[i].step], 0));
      digitalWrite(
            _status[i].pin2,
            bitRead(uln2003apg_lookup[stepper::_status[i].step], 1));
      digitalWrite(
            _status[i].pin3,
            bitRead(uln2003apg_lookup[stepper::_status[i].step], 2));
      digitalWrite(
            _status[i].pin4,
            bitRead(uln2003apg_lookup[stepper::_status[i].step], 3));

      if (stepper::_status[i].clockwise)
      {
        (++stepper::_status[i].step) %= 8;
      }
      else
        (stepper::_status[i].step += 7) %= 8;

      stepper::_status[i].count = 0;
    }

//    Serial.println(stepper::_status[i].count);
//    if (stepper::_status[i].stepper->_activated)
//      Serial.println("toto");

//    Serial.println(stepper::_status[i].stamp);
  }
}


}


ISR(__STEPPER_VECTOR)
{
  stepper::_update_uln2003apg();
}

namespace stepper
{


bool uln2003apg::module_set = false;


uln2003apg::uln2003apg(void)
  : _id(-1)
{
  for (int i = 0; i < MAX_STEPPER; ++i)
  {
    if (!_status[i].stepper)
    {
      _status[i].stepper = this;
      _status[i].stamp = -1;
      _status[i].activated = false;
      _status[i].clockwise = true;
      _id = i;
      _status_max = max(_id + 1, _status_max);
      break;
    }
  }
}


void uln2003apg::setup(int pin1, int pin2, int pin3, int pin4)
{
  if (!module_set)
  {
    setup_steppers();
    module_set = true;
  }

  _status[_id].pin1 = pin1;
  _status[_id].pin2 = pin2;
  _status[_id].pin3 = pin3;
  _status[_id].pin4 = pin4;

  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);
  pinMode(pin3, OUTPUT);
  pinMode(pin4, OUTPUT);
}


void uln2003apg::loop(void)
{
//  if (_activated)
//  {
//    if (_clockwise)
//      clockwise();
//    else
//      anticlockwise();
//  }
}


void uln2003apg::set_motor_speed(float speed) volatile
{
//  Serial.print("Speed input: ");
//  Serial.println(speed);

  int motor_speed;

  /**/ if (speed > 0)
  {
    motor_speed = 1200.f + (1.f - speed) * 3000.f;
    _status[_id].clockwise = true;
    _status[_id].activated = true;
  }
  else if (speed < 0)
  {
    motor_speed = 1200.f + (1.f + speed) * 3000.f;
    _status[_id].clockwise = false;
    _status[_id].activated = true;
  }
  else _status[_id].activated = false;

  _status[_id].stamp = motor_speed/75;

//  Serial.print("Motor speed: ");
//  Serial.println(_motor_speed);

  Serial.print("Motor stamp: ");
  Serial.println(_status[_id].stamp);

//  Serial.println(_activated);
}


void uln2003apg::setup_steppers(void)
{
  // disable interrupts while we're setting up the timer
  cli();

  // set prescalar to "/32" = 011
  __STEPPER_TIMER_PRESCALE_REGISTER = 1;//_BV(CS31) | _BV(CS30);

  // set WGM to CTC mode (010)
  // In this mode Timer3 counts up until it matches OCR3A
  // we need to use OCR3A instead of the overflow so we can interrupt
  // more often
  __STEPPER_TIMER_COUNTER_REGISTER = __STEPPER_TIMER_COUNTER_VALUE;

  // These are actual measurements from oscope:
  //   0 : 40409.6hz
  //  10 : 22730.3
  // 100 :  2475.6
  // 124 :  2000.3
  // 125 :  1984.4
  __STEPPER_TIMER_SCALE_REGISTER = -1;

  // When the OCR3A register matches the Timer3 count, cause an interrupt
  __STEPPER_TIMER_MAX_COUNTER_REGISTER = __STEPPER_TIMER_MAX_COUNTER_VALUE;

  sei();
}


} // stepper
